#include "vio/front_end/map.h"

namespace vio {

MapPointCandidates::MapPointCandidates() {}

void MapPointCandidates::NewCandidatePoint(Point::Ptr point,
                                           double depth_sigma2) {
  point->type_ = Point::TYPE_UNKNOWNM;
  boost::unique_lock<boost::mutex> lock(mutex_candidate_);
  candidates_.push_back(PointCandidate(point, point->obs_.front()));
}

MapPointCandidates::~MapPointCandidates() {
  Reset();
}

void MapPointCandidates::AddCandidatePointToFrame(Frame::Ptr frame) {
  boost::unique_lock<boost::mutex> lock(mutex_candidate_);
  PointCandidateList::iterator it = candidates_.begin();
  while (it != candidates_.end()) {
    // point -> feature -> frame
    if (it->first->obs_.front()->frame_ptr.lock() == frame) {
      // insert feature in the frame
      it->first->type_ = Point::TYPE_UNKNOWNM;
      it->first->n_failed_reproj_ = 0;
      if (auto f_ptr = it->second->frame_ptr.lock()) {
        // TODO: why frame add feature here ?
        f_ptr->AddFeature(it->second);
      }
      it = candidates_.erase(it);
    } else {
      ++it;
    }
  }
}

bool MapPointCandidates::DeleteCandidatePoint(Point::Ptr point) {
  boost::unique_lock<boost::mutex> lock(mutex_candidate_);
  for (auto it = candidates_.begin(), ite = candidates_.end(); it != ite;
       it++) {
    if (it->first == point) {
      DeleteCandidate(*it);
      candidates_.erase(it);
      return true;
    }
  }
  return false;
}

void MapPointCandidates::DeleteCandidate(PointCandidate& c) {
  // camera-rig: another frame might still be pointing to the candidate point
  // therefore, we can't delete it right now
  c.second = nullptr;
  c.first->type_ = Point::TYPE_DELETED;
  trash_points_.push_back(c.first);
}

void MapPointCandidates::RemoveFrameCandidates(Frame::Ptr frame) {
  boost::unique_lock<boost::mutex> lock(mutex_candidate_);
  auto it = candidates_.begin();
  while (it != candidates_.end()) {
    // candidate point in this frame
    if (it->second->frame_ptr.lock() == frame) {
      DeleteCandidate(*it);
      it = candidates_.erase(it);
    } else {
      ++it;
    }
  }
}

void MapPointCandidates::Reset() {
  boost::unique_lock<boost::mutex> lock(mutex_candidate_);
  std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c) {
    c.first = nullptr;
    c.second = nullptr;
  });
  candidates_.clear();
}

void MapPointCandidates::EmptyTrash() {
  std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point::Ptr& p) {
    p = nullptr;
  });
  trash_points_.clear();
}

Map::Map() {}

Map::~Map() {
  Reset();
  LOG(INFO) << "Map destructured";
}

void Map::Reset() {
  keyframes_.clear();
  point_candidates_.Reset();
  EmptyTrash();
}

bool Map::SafeDeleteFrame(Frame::Ptr frame) {
  bool found = false;
  for (auto it = keyframes_.begin(), ite = keyframes_.end(); it != ite; it++) {
    if (*it == frame) {
      std::for_each((*it)->features_.begin(),
                    (*it)->features_.end(),
                    [&](Feature::Ptr ftr) { RemovePtFrameRef(*it, ftr); });
      keyframes_.erase(it);
      found = true;
      break;
    }
  }
  point_candidates_.RemoveFrameCandidates(frame);

  if (found) {
    return true;
  }
  LOG(INFO) << "Tried to delete Keyframe in map which was not there.";
  return false;
}

void Map::RemovePtFrameRef(Frame::Ptr frame, Feature::Ptr ftr) {
  if (ftr->point_ptr.lock()) {
    // mappoint may have been deleted in a previous ref. removal
    return;
  }
  auto p_ptr = ftr->point_ptr.lock();
  if (p_ptr->obs_.size() <= 2) {
    // if the references list of mappoint has only size=2, delete mappoint
    SafeDeletePoint(p_ptr);
    return;
  }
  // remove reference from mappoint
  p_ptr->DeleteFrameRef(frame);
  // check if mp was keyMp in keyframe
  frame->RemoveKeyPoint(ftr);
}

// delete point and frame reference, clean feature obs and delete mappoint
void Map::SafeDeletePoint(Point::Ptr pt) {
  // delete references to mappoints in all keyframes
  std::for_each(pt->obs_.begin(), pt->obs_.end(), [&](Feature::Ptr ftr) {
    ftr->point_ptr.reset();
    if (auto f_ptr = ftr->frame_ptr.lock()) {
      f_ptr->RemoveKeyPoint(ftr);
    }
  });
  pt->obs_.clear();
  // delete mappoint
  DeletePoint(pt);
}

void Map::DeletePoint(Point::Ptr pt) {
  pt->type_ = Point::TYPE_DELETED;
  trash_points_.push_back(pt);
}

void Map::AddKeyFrame(Frame::Ptr new_kf) {
  keyframes_.push_back(new_kf);
}

void Map::GetCloseKeyFrames(
    const Frame::Ptr& frame,
    std::list<std::pair<Frame::Ptr, double>>& close_kfs) const {
  for (auto kf : keyframes_) {
    // check if kf has overlapping field of view with frame,
    // use therefore keypoints
    for (auto keypoint : kf->key_pts_) {
      if (keypoint == nullptr) {
        continue;
      }
      if (auto p_ptr = keypoint->point_ptr.lock()) {
        if (frame->IsVisible(p_ptr->pos_)) {
          close_kfs.push_back(std::make_pair(
              kf,
              (frame->T_f_w_.translation() - kf->T_f_w_.translation()).norm()));
          // this keyframe has an overlapping field of view -> add to close_kfs
          break;
        }
      }
    }
  }
}

Frame::Ptr Map::GetClosestKeyFrame(const Frame::Ptr& frame) const {
  std::list<std::pair<Frame::Ptr, double>> close_kfs;
  GetCloseKeyFrames(frame, close_kfs);
  if (close_kfs.empty()) {
    return nullptr;
  }
  // sort KFs with overlap according to their closeness
  close_kfs.sort(boost::bind(&std::pair<Frame::Ptr, double>::second, _1) <
                 boost::bind(&std::pair<Frame::Ptr, double>::second, _2));
  // not current query frame
  if (close_kfs.front().first != frame) {
    return close_kfs.front().first;
  }
  close_kfs.pop_front();
  return close_kfs.front().first;
}

Frame::Ptr Map::GetFurthestKeyFrame(const Eigen::Vector3d& pos) const {
  Frame::Ptr furthest_ptr = nullptr;
  double max_dist = 0.0;
  for (auto it = keyframes_.begin(), ite = keyframes_.end(); it != ite; it++) {
    double dist = ((*it)->pos() - pos).norm();
    if (dist > max_dist) {
      max_dist = dist;
      furthest_ptr = *it;
    }
  }
  return furthest_ptr;
}

bool Map::GetKeyFrameById(const int id, Frame::Ptr& frame) const {
  bool found = false;
  for (auto it = keyframes_.begin(), ite = keyframes_.end(); it != ite; it++) {
    if ((*it)->id_ == id) {
      found = true;
      frame = *it;
      break;
    }
  }
  return found;
}

void Map::Transform(const Eigen::Matrix3d& R,
                    const Eigen::Vector3d& t,
                    const double& s) {
  for (auto it = keyframes_.begin(), ite = keyframes_.end(); it != ite; it++) {
    Eigen::Vector3d pos = s * R * (*it)->pos() + t;
    Eigen::Matrix3d rot = R * (*it)->T_f_w_.rotationMatrix().inverse();
    (*it)->T_f_w_ = Sophus::SE3d(rot, pos).inverse();
    for (auto ftr = (*it)->features_.begin(); ftr != (*it)->features_.end();
         ftr++) {
      auto p_ptr = (*ftr)->point_ptr.lock();
      if (p_ptr == nullptr) {
        continue;
      }
      if (p_ptr->last_published_ts_ == -1000) {
        continue;
      }
      p_ptr->last_published_ts_ = -1000;
      p_ptr->pos_ = s * R * p_ptr->pos_ + t;
    }
  }
}

void Map::EmptyTrash() {
  std::for_each(trash_points_.begin(),
                trash_points_.end(),
                [&](Point::Ptr& pt) { pt = nullptr; });
  trash_points_.clear();
  point_candidates_.EmptyTrash();
}

}  // namespace vio
